#include "bvhtovmd.h"

BvhToVmd::BvhToVmd(QWidget *parent, Qt::WFlags flags)
	: QMainWindow(parent, flags)
{
	ui.setupUi(this);

	connect(ui.actionOpenBVH, SIGNAL(triggered()),
		this, SLOT(fileOpenBVH()));
	connect(ui.actionSaveAsVMD, SIGNAL(triggered()),
		this, SLOT(fileSaveAsVMD()));
	connect(ui.actionQuit, SIGNAL(triggered()),
		this, SLOT(fileQuit()));

}

BvhToVmd::~BvhToVmd()
{

}

/*
BVH and VMD files don't use the same units so a scaling factor is used
For iPi Studio BVH files I calculated it to be about 11.52
*/
double BvhToVmd::getModelScalingFactor()
{
	return ui.scalingFactorSpinBox->value();
}

void BvhToVmd::fileOpenBVH()
{
	QString path = QFileDialog::getOpenFileName(0,"File Open",m_bvhPath,"*.bvh");
	if (path.isEmpty()) return;

	m_bvhFile.ReadBVH(path);

	m_bvhPath = path;
}

void BvhToVmd::fileSaveAsVMD()
{
	QString vmdPath = m_vmdPath;
	if (vmdPath.isEmpty()) {
		vmdPath = m_bvhPath;
		if (!vmdPath.isEmpty()) {
			vmdPath.chop(4);
			vmdPath.append(".vmd");
		}
	}
	QString path = QFileDialog::getSaveFileName(0,"File Save",vmdPath,"*.vmd");
	if (path.isEmpty()) return;

	convertBvhToVmd();
	m_vmdFile.save(path);

	m_vmdPath = path;
}

void BvhToVmd::fileQuit()
{
	QApplication::exit(0);
}


void BvhToVmd::convertBvhToVmd()
{
	QStringList bvhBoneList, vmdBoneList;
	QList<int> bvhIndexList;
	bvhBoneList << "Hip" << "Chest" << "Neck" << "Head"
		        << "RThigh" << "RShin" << "RFoot"
				<< "LThigh" << "LShin" << "LFoot"
				<< "RClavical" << "RShoulder" << "RForearm" << "RHand"
				<< "LClavical" << "LShoulder" << "LForearm" << "LHand"
				<< "RFoot" << "LFoot";// << "RToe" << "LToe";
	vmdBoneList << "center" << "upperbody" << "neck" << "head"
		        << "leg_R" << "knee_R" << "ankle_R"
				<< "leg_L" << "knee_L" << "ankle_L"
				<< "shoulder_R" << "arm_R" << "elbow_R" << "wrist_R"
				<< "shoulder_L" << "arm_L" << "elbow_L" << "wrist_L"
				<< "legIK_R" << "legIK_L";// << "toeIK_R" << "toeIK_L";
	/* Removed toes for now because if Miku's hips are narrower than the BVH model's then
	   her toes can easily cross over each other when they aren't actually doing that
	   This should be seen with the legIK positions too to some extend, but the conversion
	   doesn't work at all without them*/
	bvhIndexList << 0 << 1 << 2        << 12 << 13 << 14     << 15 << 16 << 17     << 18 << 19 << 20
		         << 135 << 136 << 137  << 138 << 139 << 140  << 141 << 142 << 143
				 << 147 << 148 << 149  << 150 << 151 << 152  << 153 << 154 << 155
				 << 78 << 79 << 80     << 81 << 82 << 83     << 84 << 85 << 86     << 87 << 88 << 89
				 << 21 << 22 << 23     << 24 << 25 << 26     << 27 << 28 << 29     << 30 << 31 << 32
				 << 0 << 1 << 2        << 0 << 1 << 2;
	Matrix3f armMatRz = PmdBone::quat2matrix(0,0,-0.281157,0.959661);
	Matrix3f armMatRx = PmdBone::quat2matrix(0.5,0,0,0.866);
	Matrix3f armMatLz = PmdBone::quat2matrix(0,0,0.281157,0.959661);
	Matrix3f armMatLx = PmdBone::quat2matrix(0.5,0,0,0.866);
	Matrix3f armMatR = armMatRx*armMatRz;
	Matrix3f armMatL = armMatLx*armMatLz;
	Matrix3f elbowMat = PmdBone::quat2matrix(-0.5,0,0,0.866);
	Matrix3f csMat = PmdBone::quat2matrix(0,0,1,0);
	double sf = getModelScalingFactor();

	QMap<QString,Vector3f> initMap;
	m_bvhFile.setData(0);
	initMap["Hip"] = m_bvhFile.getJointFromName("Hip")->calcPosGlobal();
	initMap["RFoot"] = m_bvhFile.getJointFromName("RFoot")->calcPosGlobal();
	initMap["LFoot"] = m_bvhFile.getJointFromName("LFoot")->calcPosGlobal();
	initMap["RToe"] = m_bvhFile.getJointFromName("RToe")->calcPosGlobal();
	initMap["LToe"] = m_bvhFile.getJointFromName("LToe")->calcPosGlobal();

	m_vmdFile.m_motionMap.clear();

	m_vmdFile.setKeyFramesToWrite(m_bvhFile.getDataCount()*vmdBoneList.count());
	for (int d=0;d<bvhBoneList.count();d++) {
		QList<VMD_BoneMotion> bmList;
		for (int n=0;n<m_bvhFile.getDataCount();n++) {
			VMD_BoneMotion bm;
			bm.name = vmdBoneList.value(d);
			bm.frame = n;
			Vector3f bvhRot;
			if (bvhBoneList.value(d) == "Hip") {
				bm.rot = Vector4f::Zero();
				bm.pos << ( m_bvhFile.getDataValue(n,bvhIndexList.value(d*3)) - initMap["Hip"](0) )/sf, 
					      ( m_bvhFile.getDataValue(n,bvhIndexList.value(d*3+1)) - initMap["Hip"](1) )/sf, 
						  -( m_bvhFile.getDataValue(n,bvhIndexList.value(d*3+2)) - initMap["Hip"](2) )/sf;
			} else if (vmdBoneList.value(d).contains("IK")) {
				bm.rot = Vector4f::Zero();
				m_bvhFile.setData(n);
				BvhJoint* jointPtr = m_bvhFile.getJointFromName(bvhBoneList.value(d));
				Vector3f posVec = jointPtr->calcPosGlobal();
				bm.pos << (posVec(0)-initMap[bvhBoneList.value(d)](0))/sf,
					      (posVec(1)-initMap[bvhBoneList.value(d)](1))/sf,
						  -(posVec(2)-initMap[bvhBoneList.value(d)](2))/sf;
			} else {
				bvhRot << m_bvhFile.getDataValue(n,bvhIndexList.value(d*3)),
						  m_bvhFile.getDataValue(n,bvhIndexList.value(d*3+1)),
						  m_bvhFile.getDataValue(n,bvhIndexList.value(d*3+2));
				Matrix3f tMat = BvhJoint::calcRotMatrixLocal(bvhRot(0),bvhRot(1),bvhRot(2));
				Matrix3f rotMat, combMat;
				rotMat = csMat*tMat*csMat;
				Vector4f vmdRot;

				bm.pos = Vector3f::Zero();

				if (vmdBoneList.value(d) == "arm_R") {
					combMat = rotMat*armMatR;
				} else if (vmdBoneList.value(d) == "arm_L") {
					Matrix3f armMat = PmdBone::quat2matrix(0,0,0.281157,0.959661);
					combMat = rotMat*armMatL;
				} else if (vmdBoneList.value(d) == "elbow_L"
				        || vmdBoneList.value(d) == "elbow_R") {
					combMat = rotMat;
				} else {
					combMat = rotMat;
				}
				vmdRot = calcQuatFromMatrix(combMat);
				bm.rot = vmdRot;
			}
			bmList.append(bm);
		}
		m_vmdFile.m_motionMap[vmdBoneList.value(d)] = bmList;
	}
}

// Calculates quaternions from 3x3 matrix
Vector4f BvhToVmd::calcQuatFromMatrix(const Matrix3f& mat)
{
	double e1, e2, e3, e4;
	Vector4f vec;
	e4 = sqrt( (mat.trace() + 1.0)/4.0 );
	if (e4 != 0) {
		e1 = (mat(2,1)-mat(1,2)) / (4.0*e4);
		e2 = (mat(0,2)-mat(2,0)) / (4.0*e4);
		e3 = (mat(1,0)-mat(0,1)) / (4.0*e4);
	} else {
		e1 = sqrt( (1+2*mat(0,0)-mat.trace())/4 );
		e2 = sqrt( (1+2*mat(1,1)-mat.trace())/4 );
		e3 = sqrt( (1+2*mat(2,2)-mat.trace())/4 );
		if (e1 != 0) {
			e2 = (mat(1,0)+mat(0,1)) / (4.0*e1);
			e3 = (mat(2,0)+mat(0,2)) / (4.0*e1);
		} else if (e2 != 0) {
			e1 = (mat(1,0)+mat(0,1)) / (4.0*e2);;
			e3 = (mat(2,1)+mat(1,2)) / (4.0*e2);;
		} else if (e3 != 0) {
			e1 = (mat(2,0)+mat(0,2)) / (4.0*e3);;
			e2 = (mat(2,1)+mat(1,2)) / (4.0*e3);;
		}
	}
	vec << e1,e2,e3,e4;
	return vec;
}